
#include "remote_TASK.h"
#include "rtos_queues.h"
#include "state.h"
#include "timers.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "rtos_queues.h"
#include "driverSD_SPI_DMA_ISR_TASK.h"
#include "tools.h"
#include "bode.h"
#include "qmath.h"
#include "commands.h"

volatile unsigned char monitoringBusy = 0;
TimerHandle_t monitoringTimer;


BYTE remoteBuffer[256];
int remoteParamLength = 0;


void cmdConfigValue()
{
    void * value;
    char * tab;
    char * name;

    int pos = 0;

    tab = &remoteBuffer[0];
    while (remoteBuffer[pos++] != 0x00);

    name = &remoteBuffer[pos];
    while (remoteBuffer[pos++] != 0x00);

    value = &remoteBuffer[pos];

    configSetValue(tab,name,value);
}

void cmdConfigReadSD()
{
    extern QueueHandle_t queSD;
    struct SdCommand_t read;
    read.type = SDCMD_READ_CONFIG;
    xQueueSendToBack(queSD,&read,0);
}

void cmdConfigWriteSD()
{
    extern QueueHandle_t queSD;
    struct SdCommand_t write;
    write.type = SDCMD_WRITE_CONFIG;
    xQueueSendToBack(queSD,&write,0);
}

void cmdDebugRc()
{
    if (remoteParamLength != 2 * 8) return;

    int i;
    for (i=0;i<2*8; i+=2)
    {
        STATE.rc.axis[i/2] = (remoteBuffer[i] << 8) | (remoteBuffer[i+1]);
        STATE.rc.axisQ[i/2] = STATE.rc.axis[i/2] << 6;
    }

    logStore(xTaskGetTickCount(), LOG_ID_RC_INPUT, STATE.rc.axis);
}

void cmdSdFormat()
{
    extern QueueHandle_t queSD;
    struct SdCommand_t format;
    format.type = SDCMD_FATFORMAT;
    xQueueSendToBack(queSD,&format,0);
}

void cmdSdRawRead()
{
    extern QueueHandle_t queSD;
    struct SdCommand_t sdRaw;
    int sector = INT_FROM_BYTES(remoteBuffer,0);

    sdRaw.ref = (void*)sector;
    sdRaw.type = SDCMD_RAW_READ;

    if (xQueueSendToBack(queSD, &sdRaw, 0) != pdPASS)
    {
        portNOP();
    }
}

void cmdLogSetInterval()
{
    int i;
    for (i=0;i<remoteParamLength;i+=3)
    {
        STATE.btLogging.varEnabled[remoteBuffer[i]] = (remoteBuffer[i+1] << 8) | (remoteBuffer[i+2]);
    }
}

void cmdLogSendDefinitions()
{
    logSendDefinitions();
}

void cmdConfigSendAll()
{
    configSendAll();
}

unsigned int cmdMonitoringSendString(unsigned char * str, unsigned char * buffer, unsigned int pos)
{
    do
    {
        buffer[pos++] = *(str);
    }
    while (*(str++) != 0x00);

    return pos;
}

unsigned int cmdMonitoringSendInt(unsigned int val, unsigned char * buffer, unsigned int pos)
{
    buffer[pos++] = 1;
    INT_TO_BYTES_INC(val, buffer, pos);
    return pos;
}

void cmdMonitoringSend()
{
    monitoringBusy = 1;

    if (!rnClaimTx(configTICK_RATE_HZ))
    {
        monitoringBusy = 0;
        return;
    }

    BYTE * packet = rnGetTxBuffer();
    unsigned int pos = 0;
    // definition
    packet[pos++] = CMD_MONITORING_SEND;
    pos = cmdMonitoringSendString("TIME", packet, pos);
    pos = cmdMonitoringSendInt(xTaskGetTickCount(), packet, pos);
    pos = cmdMonitoringSendString("T_QUAD", packet, pos);
    pos = cmdMonitoringSendInt(STATE.monitoring.quadTaskTickCount, packet, pos);
    pos = cmdMonitoringSendString("T_SD",packet, pos);
    pos = cmdMonitoringSendInt(STATE.monitoring.driverSDTickCount, packet, pos);
    pos = cmdMonitoringSendString("T_RN",packet, pos);
    pos = cmdMonitoringSendInt(STATE.monitoring.driverRN41TickCount, packet, pos);
    pos = cmdMonitoringSendString("T_STAB",packet, pos);
    pos = cmdMonitoringSendInt(STATE.monitoring.stabilityTaskTickCount, packet, pos);
    pos = cmdMonitoringSendString("S_MAGN",packet, pos);
    pos = cmdMonitoringSendInt(STATE.hmc5883.totalSamples, packet, pos);
    pos = cmdMonitoringSendInt(STATE.hmc5883.failures, packet, pos);
    pos = cmdMonitoringSendString("S_IMU", packet, pos);
    pos = cmdMonitoringSendInt(STATE.mpu6050.totalSamples, packet, pos);
    pos = cmdMonitoringSendInt(STATE.mpu6050.failures, packet, pos);
    pos = cmdMonitoringSendString("S_BARO",packet, pos);
    pos = cmdMonitoringSendInt(STATE.mpl3115.totalSamples, packet, pos);
    pos = cmdMonitoringSendInt(STATE.mpl3115.failures, packet, pos);
    pos = cmdMonitoringSendString("SD_STATUS",packet, pos);
    pos = cmdMonitoringSendInt(STATE.sdLogging.freeBlocks * SECTORS_PER_BLOCK, packet, pos);
    pos = cmdMonitoringSendInt(STATE.sd.sizeSectors, packet, pos);
    pos = cmdMonitoringSendInt(STATE.sd.sizeWrittenSectors, packet, pos);
    pos = cmdMonitoringSendString("V_SENSE",packet, pos);
    pos = cmdMonitoringSendInt(STATE.monitoring.vLipo, packet, pos);
    pos = cmdMonitoringSendInt(STATE.monitoring.current, packet, pos);
    pos = cmdMonitoringSendString("_BENCH",packet,pos);
    extern volatile unsigned int bench_idle_time;
    extern volatile unsigned int bench_idle_time_prev;
    
    bench_idle_time_prev = bench_idle_time;
    bench_idle_time = 0;
    pos = cmdMonitoringSendInt(bench_idle_time_prev, packet, pos);
    //rnTransmitAndConfirmBlock(packet, pos);

    //rnReleaseTx();

    rnTransmitAndRelease(packet, pos);
    monitoringBusy = 0;
}

void cmdMonitoringSet()
{
    if (remoteParamLength == 2)
    {
        unsigned short freq = (remoteBuffer[0] << 8) | remoteBuffer[1];
        if (freq > configTICK_RATE_HZ) freq = configTICK_RATE_HZ;
        if (freq == 0)
        {
            xTimerStop(monitoringTimer, 0);
        }
        else
        {
            xTimerChangePeriod(monitoringTimer, configTICK_RATE_HZ / freq,0 );
            xTimerStart(monitoringTimer, 0);
        }
    }
}


void cmdLogRetrieveAll()
{
    // request start/end sector for log
    struct SdCommandFileRange_t getRange;
    getRange.runID = remoteBuffer[0];
    getRange.done = 0;

    extern QueueHandle_t queSD;
    if (xQueueSendToBack(queSD, &getRange, portMAX_DELAY) != pdPASS)
        return;

    while (getRange.done == 0)
    {
        vTaskDelay(1);
    }

    int i, j;
    if (getRange.blockStart >= getRange.blockEnd) return;

    int btDisable = STATE.btLogging.disableSending;
    STATE.btLogging.disableSending = 1;

    // TODO: Move to SD task.
    for (i = getRange.blockStart; i < getRange.blockEnd; i++)
    {
        for (j=0;j<2;j++)
        {
            /*struct PriorityPacket_t * packet = rnClaimPriorityPacketBlock();

            struct sdCommandReadSector_t read;
            packet->packet[0] = CMD_LOG_RETRIEVE_ALL;
            packet->packet[1] = j;
            INT_TO_BYTES(i,packet->packet, 2);
            INT_TO_BYTES(getRange.blockEnd,packet->packet, 6);

            read.buffer = &packet->packet[10];
            read.sector = BLOCK_TO_SECTOR(i) + (j * 4);
            read.length = 4;
            while (xQueueSendToBack(queSD, &getRange, portMAX_DELAY) != pdPASS);
            while (read.done == 0) vTaskDelay(1);

            packet->length = 10 + 2048;
            rnSendPriorityPacket(packet);
             */
        }
        vTaskDelay(10);
    }
    STATE.btLogging.disableSending = btDisable;
}

void cmdLogRetrievePartial()
{

}

void remoteMonitoringTimer(TimerHandle_t timer)
{
    if (monitoringBusy) return;
    struct CommandIn_t cmd;
    cmd.command = 0x02;
    commandPost(&cmd);
}

void TaskRemote(void * params)
{
    struct CommandIn_t cmd;

    monitoringTimer = xTimerCreate("", configTICK_RATE_HZ, pdTRUE,NULL, remoteMonitoringTimer);
    extern QueueHandle_t queRemoteCommands;
    for(;;)
    {
        if (xQueueReceive(queRemoteCommands, &cmd, portMAX_DELAY) != pdTRUE) continue;

        void (* func)() = commandGetFunction(cmd.command);

        if (func != NULL)
        {
            int i = 0;

            for (i=0; i < cmd.paramLength; i++)
                remoteBuffer[i] = rnGetParam(cmd.paramPosition + i);
            remoteParamLength = cmd.paramLength;

            (*func)();
        }
    }
}
